/**********************************************************************************************************************
 *
 * Project:    mGameEngine
 * File:       quaternion.h
 * Author:     karooolek
 * Created on: Jun 23, 2009
 *
 **********************************************************************************************************************/

#ifndef QUATERNION_H_
#define QUATERNION_H_

#include "vector3.h"

namespace mGameEngine
{

/**
 * Quaternion.
 */
struct Quaternion
{
    /**
     * Quaternion components
     */
    float x, y, z, w;

    /**
     * Create quaternion.
     */
    Quaternion() : x(0.0f), y(0.0f), z(0.0f), w(1.0f)
    {
    }

    /**
     * Create quaternion with initial components.
     * @param x X component.
     * @param y Y component.
     * @param z Z component.
     * @param w W component.
     */
    Quaternion(float x, float y, float z, float w) :
        x(x), y(y), z(z), w(w)
    {
    }

    /**
     * Create quaternion from rotation axis and angle.
     * @param axis rotation axis.
     * @param angle rotation angle in radians
     */
    Quaternion(const Vector3 &axis, float angle)
    {
        Vector3 a = axis;
        a.normalize();

        angle *= 0.5f;
        float s = sinf(angle);
        x = a.x * s;
        y = a.y * s;
        z = a.z * s;
        w = cos(angle);
    }

    /**
     * Create quaternion from euler angles.
     * @param yaw yaw (heading)
     * @param pitch pitch
     * @param roll roll
     */
    Quaternion(float yaw, float pitch, float roll) :
        x(0.0f), y(0.0f), z(0.0f), w(1.0f)
    {
        rotateZ(yaw);
        rotateY(pitch);
        rotateX(roll);
    }

    /**
     * Create quaternion from direction.
     * @param dir direction.
     */
    Quaternion(const Vector3 &dir) :
        x(0.0f), y(0.0f), z(0.0f), w(1.0f)
    {
        Vector3 a = dir;
        a.normalize();

        rotateZ(atan2(a.y, a.x));
        rotateY(asin(-a.z));
    }

    /**
     * Copy constructor.
     * @param q quaternion to copy.
     */
    Quaternion(const Quaternion &q) : x(q.x), y(q.y), z(q.z), w(q.w)
    {
    }

    /**
     * Assign quaternion.
     * @param q quaternion to assign.
     * @return quaternion with assigned value.
     */
    Quaternion &operator=(const Quaternion &q)
    {
        x = q.x;
        y = q.y;
        z = q.z;
        w = q.w;
        return *this;
    }

    /**
     * Negate quaternion.
     * @return negated quaternion.
     */
    Quaternion operator-()
    {
        return Quaternion(-x, -y, -z, -w);
    }

    /**
     * Multiply by quaternion.
     * @param q multiplier
     * @return multiplied quaternion.
     */
    Quaternion &operator*=(const Quaternion &q)
    {
        float qx(x), qy(y), qz(z);
        x = w * q.x + qx * q.w + qy * q.z - qz * q.y;
        y = w * q.y + qy * q.w + qz * q.x - qx * q.z;
        z = w * q.z + qz * q.w + qx * q.y - qy * q.x;
        w = w * q.w - qx * q.x - qy * q.y - qz * q.z;
        return *this;
    }

    /**
     * Multiply quaternions.
     * @param q multiplier
     * @return multiplied quaternions.
     */
    Quaternion operator*(const Quaternion &q) const
    {
        return Quaternion(w * q.x + x * q.w + y * q.z - z * q.y,
                          w * q.y + y * q.w + z * q.x - x * q.z,
                          w * q.z + z * q.w + x * q.y - y * q.x,
                          w * q.w - x * q.x - y * q.y - z * q.z);
    }

    /**
     * Multiply quaternion by vector.
     * @param v vector multiplier.
     * @return quaternion multiplied by vector - rotated vector.
     */
    Vector3 operator*(const Vector3 &v) const
    {
        float xx(x*x), yy(y*y), zz(z*z);
        float xy(x*y), xz(x*z), yz(y*z), wx(w*x), wy(w*y), wz(w*z);

        return Vector3((1.0f - 2.0f * (yy + zz)) * v.x + (2.0f * (xy - wz)) * v.y + (2.0f * (xz + wy)) * v.z,
                       (2.0f * (xy + wz)) * v.x + (1.0f - 2.0f * (xx + zz)) * v.y + (2.0f * (yz - wx)) * v.z,
                       (2.0f * (xz - wy)) * v.x + (2.0f * (yz + wx)) * v.y + (1.0f - 2.0f * ( xx + yy )) * v.z);
    }

    /**
     * Invert quaternion.
     * @return inverted quaternion.
     */
    Quaternion &invert()
    {
        float n = 1.0f / length(*this);
        x *= -n;
        y *= -n;
        z *= -n;
        w *= n;
        return *this;
    }

    /**
     * Normalize quaternion.
     * @return normalized quaternion
     */
    Quaternion &normalize()
    {
        float n = 1.0f / length(*this);
        x *= n;
        y *= n;
        z *= n;
        w *= n;
        return *this;
    }

    /**
     * Rotate quaternion around specified axis.
     * @param axis normalized rotation axis.
     * @param angle rotation angle in radians.
     */
    void rotate(const Vector3 &axis, float angle)
    {
        if(angle != 0.0f)
        {
            *this *= Quaternion(axis, angle);
        }
    }

    /**
     * Rotate quaternion around X axis.
     * @param angle rotation angle in radians.
     */
    void rotateX(float angle)
    {
        if(angle != 0.0f)
        {
            angle *= 0.5f;
            *this *= Quaternion(sin(angle), 0.0f, 0.0f, cos(angle));
        }
    }

    /**
     * Rotate quaternion around Y axis.
     * @param angle rotation angle in radians.
     */
    void rotateY(float angle)
    {
        if(angle != 0.0f)
        {
            angle *= 0.5f;
            *this *= Quaternion(0.0f, sin(angle), 0.0f, cos(angle));
        }
    }

    /**
     * Rotate quaternion around Z axis.
     * @param angle rotation angle in radians.
     */
    void rotateZ(float angle)
    {
        if(angle != 0.0f)
        {
            angle *= 0.5f;
            *this *= Quaternion(0.0f, 0.0f, sin(angle), cos(angle));
        }
    }

    /**
     * Get quaternion orientation axis.
     * @return orientation axis.
     */
    Vector3 getAxis() const
    {
        float s = 1.0f - w * w;

        if(s <= 0.0f)
        {
            return Vector3(0.0f, 0.0f, 0.0f);
        }

        return Vector3(x, y, z) / sqrtf(s);
    }

    /**
     * Get quaternion orientation angle.
     * @return orientation angle in radians.
     */
    float getAngle() const
    {
        return 2.0f * acosf(clamp(w, -1.0f, 1.0f));
    }

    /**
     * Get quaternion orientation yaw.
     * @return orientation yaw in radians.
     */
    float getYaw() const
    {
        return atan2f(w*z + x*y, 0.5f - y*y - z*z);
    }

    /**
     * Get quaternion orientation pitch.
     * @return orientation pitch in radians.
     */
    float getPitch() const
    {
        return asinf(2.0f * (w*y - z*x));
    }

    /**
     * Get quaternion orientation roll.
     * @return orientation roll in radians.
     */
    float getRoll() const
    {
        return atan2f(w*x + y*z, 0.5f - x*x + y*y);
    }

    /**
     * Calculate dot product of two quaternions.
     * @param q first quaternion.
     * @param p second quaternion.
     * @return dot producy of two quaternions.
     */
    friend float dot(const Quaternion &q, const Quaternion &p)
    {
        return q.x*p.x + q.y*p.y + q.z*p.z + q.w*p.w;
    }

    /**
     * Calculate length of quaternion.
     * @param q quaternion.
     * @return quaternions length.
     */
    friend float length(const Quaternion &q)
    {
        return sqrtf(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w);
    }

    /**
     * Spherical linear interpolation between two quaternions.
     * @param q first quaternion.
     * @param p second quaternion
     * @param t interpolation factor.
     * @return linearly interpolated quaternion between q and p.
     */
    friend Quaternion slerp(const Quaternion &q, const Quaternion &p, float t)
    {
        float cosw = dot(q, p);
        Quaternion tmp = p;

        if(cosw < 0.0f)
        {
            tmp = -tmp;
            cosw = -cosw;
        }

        float k0, k1;
        if(cosw >= 1.0f)
        {
            k0 = 1.0f - t;
            k1 = t;
        }
        else
        {
            float sinw = sqrtf(1.0f - cosw*cosw);
            float w = atan2f(sinw, cosw);
            float sinw1 = 1.0f / sinw;
            k0 = sinf((1.0f - t) * w) * sinw1;
            k1 = sinf(t * w) * sinw1;
        }

        return Quaternion(q.x * k0 + tmp.x * k1,
                          q.y * k0 + tmp.y * k1,
                          q.z * k0 + tmp.z * k1,
                          q.w * k0 + tmp.w * k1);
    }
};

}

#endif // QUATERNION_H_
